#include "./SYSTEM/sys/sys.h"
#include "./SYSTEM/usart/usart.h"
#include "./SYSTEM/delay/delay.h"
#include "./BSP/OLED/OLED.h"
#include "./BSP/DMP/inv_mpu.h"
#include "./BSP/IIC/myiic.h"
#include "./BSP/MPU6050/mpu6050.h"
float pitch,roll,yaw;
uint8_t ret;
int main()
{
    HAL_Init();
    delay_init(168);
    sys_stm32_clock_init(336,8,2,7);
    usart_init(115200);
    OLED_Init();

    MyIIC_init();

    OLED_ShowString(1,1,"Hello world",OLED_8X16);
    OLED_Printf(0,32,OLED_8X16,"Hello if");
    OLED_Update();

    do
    {
       ret = MPU6050_DMP_init();
       OLED_Printf(0,0,OLED_8X16,"%d",ret);
       OLED_Update();
    } while (ret);

    while(1)
    {
        ret = MPU6050_DMP_Get_Date(&pitch, &roll, &yaw);
        OLED_Printf(0, 0, OLED_8X16, "Pitch:%.2f", pitch);
        OLED_Printf(0, 16, OLED_8X16, "Roll:%.2f", roll);
        OLED_Printf(0, 32, OLED_8X16, "Yaw:%.2f", yaw);
        OLED_Update();
        printf("\r\n%.2f,%.2f,%.2f\r\n", pitch, roll, yaw);
        delay_ms(100);
    }
}

